#ifdef GUI
#include <opencv2/opencv.hpp>
#include <vector>
#include <set>
#include "display.h"
#include "ekf.h"

using namespace cv;
using namespace std;

typedef struct {
	double p;
	double theta;
	int id;
} measure;

class Simulator {
	MapDisplay *disp;
	EkfSlam *slam;
	RNG r;

	set<int> addedFeatures;
	// will hold features that have been added to slam
public:
	double r_x, r_y, r_theta, r_fov;

	vector<feature> map; // will hold virtual map
	Simulator(Mat q, Mat s);
	void step(Mat u);
	~Simulator();
};

#endif
